
import lejos.nxt.*;
import lejos.robotics.navigation.*;
import lejos.robotics.navigation.Move.MoveType;
import lejos.robotics.localization.MCLParticleSet;
import lejos.robotics.mapping.*;
import lejos.geom.Line;
import lejos.geom.Rectangle;



public class ParticleSquare {
	
	static double SIDE_SIZE = 40.0;
	static double WHEEL_DIAMETER = 5.6f;
	static double AXIS_LENGTH = 11.2f;
	
	static float SQUARE_LENGTH = 50;
	
	public static void main(String[] args) throws InterruptedException {
		
		DifferentialPilot navigator = new DifferentialPilot(WHEEL_DIAMETER, AXIS_LENGTH, Motor.A, Motor.C);
		
		// We set the parameters to smooth the trajectory:
		navigator.setRotateSpeed(100);
		navigator.setTravelSpeed(100);
		navigator.setAcceleration(200);
		
		// Initialize the particle set:
		Line[] lines = {
				new Line(0,0,0,100),
				new Line(0,100,100,100),
				new Line(100,100,100,0),
				new Line(100,0,0,0)};

		MCLParticleSet particles = new MCLParticleSet(new LineMap(lines, new Rectangle(0, 0, 100, 100)), 100, 1);
		particles.setDistanceNoiseFactor(0.05f);
		particles.setAngleNoiseFactor(0.05f);
		for (int i = 0; i < particles.numParticles(); i++)
		{
			particles.getParticle(i).getPose().setLocation((100-SQUARE_LENGTH)/2, (100-SQUARE_LENGTH)/2);
			particles.getParticle(i).getPose().setHeading(0);
		}
		
		Thread.sleep(1000);
		
		showParticles(particles);
		//Forward - stop - rotate
		for (int i = 0; i < 4; i++)
		{
			float deltaD = 0;
			float deltaA = 0;
			float lastD  = 0;
			float lastA  = 0;
			Move movement;
			Move aux = new Move(0, 0, true);
			
			navigator.travel(SQUARE_LENGTH, true);

			do
			{
				movement = navigator.getMovement();
				deltaD = movement.getDistanceTraveled() - lastD;
				lastD  = movement.getDistanceTraveled();
				aux.setValues(MoveType.TRAVEL, deltaD, 0, true);
				particles.applyMove(aux);
				showParticles(particles);
			} while (movement.isMoving());

			particles.resample();
			showParticles(particles);
			navigator.rotate(90, true);
			
			deltaA = 0;
			lastA  = 0;
			do
			{
				movement = navigator.getMovement();
				deltaA = movement.getAngleTurned() - lastA;
				lastA  = movement.getAngleTurned();
				aux.setValues(MoveType.ROTATE, 0, deltaA, true);
				particles.applyMove(aux);
				showParticles(particles);
			} while (movement.isMoving());
			particles.resample();
			showParticles(particles);
		}
		
		Button.waitForPress();
	}
	
	private static void showParticles(MCLParticleSet particles) throws InterruptedException
	{
		int offset = (LCD.SCREEN_WIDTH - LCD.SCREEN_HEIGHT) / 2;
		for (int i = 0; i < particles.numParticles(); i++)
		{
			int x = (int)(particles.getParticle(i).getPose().getX()/100 * LCD.SCREEN_HEIGHT);
			int y = (int)(particles.getParticle(i).getPose().getY()/100 * LCD.SCREEN_HEIGHT);
			LCD.setPixel(x + offset, y, 1);
		}
		
	}
}

